#include <ecio.h>
#include <cassert>

EcIO::EcIO(){ }

EcIO::EcIO(ec_master_t* master, int index):Slave(master, index){ }

EcIO::~EcIO(){ }

bool EcIO::Parse(EcXml &ecxml, ec_master_t* master, int index){
    // 這裏需要修改成專門去解析IO的接口
    int moduleID = 0;
    size_t size = 0;

    // IO的解析分为modules 标签解析和正常的servo标签解析
    if (ecxml.IsModule(m_SlvInfo_)) {
        for(int i = 0; i < 2; ++ i){
            ecrt_master_sdo_upload(master, index, 0xF050, i + 1, (uint8_t*)&moduleID, sizeof(moduleID), &size, NULL);
            if(!ecxml.GetModuleSyncInfo(&m_SyncInfo_, std::to_string(moduleID).c_str(), i)){
                if(m_SyncInfo_ != NULL){
                    delete[] m_SyncInfo_;
                }
                m_SyncInfo_ = NULL;
                return false;
            }
        }
    } else {
        if(!ecxml.GetServoSyncInfo(&m_SyncInfo_)) {
            if(m_SyncInfo_ != NULL){
                delete[] m_SyncInfo_;
            }
            m_SyncInfo_ = NULL;
            return false;                
        }
    }

    // 表明这个设备不可用(空设备)
    if(m_SyncInfo_[0].n_pdos == 0 && m_SyncInfo_[1].n_pdos == 0){
        m_Used_ = false;
    }
    return true;
}

void EcIO::SetPos(int32_t target) {
    rxpdo_.pos = target;
}

uint32_t EcIO::GetAnalog(int32_t pdo)
{
    if (txpdo_.ana.find(pdo) == txpdo_.ana.end())
        return 0x00;

    return txpdo_.ana[pdo];
}

uint32_t EcIO::GetDigital()
{
    return txpdo_.dig;
}

void EcIO::Retrieve(uint8_t* domainPtr) { }

void EcIO::Publisher(uint8_t* domainPtr) { }
